#include<iostream>
#include<fstream>
#include<iomanip>
#include "Frame.h"
#include "Kdtree_search.h"
#include "chrono"
#include "ransac.h"
#include "triangle.h"
#include "pose_estimate_3d2d.h"
#include "MapPoints.h"
#include "ceresBundle.h"
#include "pcl_viewer.h"

#include <nlohmann/json.hpp>
#include <fstream>

using namespace std;
using namespace Eigen;
using json = nlohmann::json;

vector<Relocation_Slam::Frame> read_images_in_folder(cv::String pattern,Eigen::Isometry3d world_camera);
Eigen::Isometry3d poseTransform();

int main()
{
    cv::String pattern = "/home/ubuntu/data/image/*.jpg";
    Eigen::Isometry3d world_camera = poseTransform();
    vector<Relocation_Slam::Frame> frames = read_images_in_folder(pattern,world_camera);

    vector<cv::Point3f> m_pOriPtXYZ;

    cv::Mat K = ( cv::Mat_<double> ( 3,3 ) << 420, 0, 320, 0, 420,240, 0, 0, 1 );

    cv::Ptr<cv::DescriptorMatcher> matcher  = cv::DescriptorMatcher::create ( "BruteForce-Hamming" );
    vector<cv::DMatch> match;
    vector<cv::DMatch> matches;
    vector<Relocation_Slam::Frame*> relo_frames;
    int frame_id = 1;
    for(int i=0;i<frames.size();i=i+3){
//        if(pow(frames[i].mPose.translation().x()-frames[250].mPose.translation().x(),2)
//            +pow(frames[i].mPose.translation().y()-frames[250].mPose.translation().y(),2)
//            +pow(frames[i].mPose.translation().z()-frames[250].mPose.translation().z(),2)<5){
                frames[i].SetFrameId(frame_id);
                Relocation_Slam::Frame* f = &frames[i];
                relo_frames.push_back(f);
                frame_id++;
//        }
    }

    std::cout<<"relo_frames.size::"<<relo_frames.size()<<std::endl;
    vector<Relocation_Slam::MapPoint> MapPoints;
    //每帧和其前面所有帧进行三角化
    for(int i=1;i<relo_frames.size();i++){
        for(int j=0;j<i;j++){ //j 表示 前面的帧
            vector<pair<int,int>> match_points; ///< 存储两帧匹配的特征点索引

            std::cout<<"hhhhh::"<<relo_frames[i]->mFeatVec.size()<<std::endl;//81

            DBoW2::FeatureVector::iterator Fit = relo_frames[i]->mFeatVec.begin();
            while(Fit!=relo_frames[i]->mFeatVec.end()){
                std::cout<<Fit->second.size()<<",";
                Fit++;
            }

            int point_matches =  SelectByBoW(*relo_frames[i],*relo_frames[j],match_points,1);
            vector< cv::Point3f > points_r;

            vector<pair<int,int>> match_points1;
            if(match_points.size()>15){
                RansacSelect(match_points,relo_frames[i]->mvKeyPoints,relo_frames[j]->mvKeyPoints,
                             match_points1);

//                std::cout<<"matches::"<<match_points.size()<<",,,,"<<match_points1.size()<<std::endl;
                if(match_points1.size()>15)
                    triangulation(relo_frames[i],relo_frames[j],K,match_points1,points_r); ///< 进行三角化
            }


            if(match_points1.size()>15){

                for(int m=0;m<match_points1.size();m++){

                    if(fabs(points_r[m].x)<15 && fabs(points_r[m].y)<15&&fabs(points_r[m].z)<15) {
                        if (relo_frames[j]->mbKpTriangle[match_points1[m].second] == false
                            &&
                            relo_frames[i]->mbKpTriangle[match_points1[m].first] == false) { ///< 若之前帧和当前帧匹配的特征点没被三角化过
                            cv::Mat p = (cv::Mat_<float>(3, 1) << points_r[m].x, points_r[m].y, points_r[m].z);
                            Relocation_Slam::MapPoint mp(p, relo_frames[i], points_r.size(),
                                                         match_points1[m].first); ///< 创建地图点并添加当前帧观测
                            mp.AddObservation(relo_frames[j], match_points1[m].second); ///< 为地图点添加之前帧观测
                            relo_frames[i]->mnMappointId[match_points1[m].first] = MapPoints.size(); ///< 为特征点绑定地图点
                            relo_frames[j]->mnMappointId[match_points1[m].second] = MapPoints.size();
                            relo_frames[i]->mbKpTriangle[match_points1[m].first] = true; ///< 设置已经三角化出地图点
                            relo_frames[j]->mbKpTriangle[match_points1[m].second] = true;
                            MapPoints.push_back(mp);
                        } else if (relo_frames[j]->mbKpTriangle[match_points1[m].second] == true
                                   && relo_frames[i]->mbKpTriangle[match_points1[m].first] ==
                                      false) { ///< 之前帧的特征点已经三角化过,需要为该地图点添加当前帧对应特征点作为观测
                            int point_id = relo_frames[j]->mnMappointId[match_points1[m].second]; ///< 之前帧的特征点对应的地图点ID
                            MapPoints[point_id].AddObservation(relo_frames[i],
                                                               match_points1[m].first); ///< 为地图点添加当前帧的观测
                            relo_frames[i]->mnMappointId[match_points1[m].first] = relo_frames[j]->mnMappointId[match_points1[m].second]; ///< 当前帧的特征点绑定地图点
                            relo_frames[i]->mbKpTriangle[match_points1[m].first] = true; ///< 当前帧的特征点已经三角化
                        } else if (relo_frames[j]->mbKpTriangle[match_points1[m].second] == false
                                   && relo_frames[i]->mbKpTriangle[match_points1[m].first] ==
                                      true) { ///< 当前帧的特征点已经三角化过,需要为该地图点添加之前帧对应特征点作为观测
                            int point_id = relo_frames[i]->mnMappointId[match_points1[m].first]; ///< 当前帧的特征点对应的地图点ID
                            MapPoints[point_id].AddObservation(relo_frames[j],
                                                               match_points1[m].second); ///< 为地图点添加之前帧的观测
                            relo_frames[j]->mnMappointId[match_points1[m].second] = relo_frames[i]->mnMappointId[match_points1[m].first]; ///< 之前帧的特征点绑定地图点
                            relo_frames[j]->mbKpTriangle[match_points1[m].second] = true; ///< 之前帧的特征点已经三角化
                        }
                    }
                }
            }

        }
    }
    std::cout<<"AllMapPoints.SIZE:::"<<MapPoints.size()<<std::endl;

    SolveProblem(relo_frames,MapPoints);

    vector<pair<int,int>> match_points2; ///< 存储两帧匹配的特征点索引
    int point_matches =  SelectByBoW(*relo_frames[488],*relo_frames[488],match_points2,1);

    std::cout<<"match_points2::"<<match_points2.size()<<std::endl;

    cv::Mat R;
    cv::Mat t;
    poseEstimate3d2d(
            *relo_frames[488],
            *relo_frames[488],
            match_points2,
            MapPoints,
            K,
            R,
            t);


    cv::Mat Tinv = (cv::Mat_<double> (4,4) <<
            R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0),
            R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0),
            R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0),
            0,0,0,1);
    cout<<"Tinv:"<<endl<<Tinv<<endl;
    cv::Mat T = Tinv.inv();
    cout<<"T:"<<endl<<T<<endl;
//
    std::cout<<"img_frame.T.groundTruth:"<<endl<<relo_frames[488]->mPose.matrix()<<endl;


//    for(int i=0;i<MapPoints.size();i=i+20){
//        std::cout<<MapPoints[i].mPose[0]<<","<<MapPoints[i].mPose[1]<<","<<MapPoints[i].mPose[2]<<std::endl;
//    }
//
//    std::cout<<"MapPoints.size()::"<<MapPoints.size()<<std::endl;


//    for(int i=0;i<MapPoints.size();i++){
//        int a = 0;
//        if(MapPoints[i].mObservations.size()>2){
//            std::map<Relocation_Slam::Frame*,int>::iterator it = MapPoints[i].mObservations.begin();
//            for(;it!=MapPoints[i].mObservations.end();it++){
//                Relocation_Slam::Frame* f =  it->first;
//                cv::Mat g;
//                f->image.copyTo(g);
//                cv::circle(g,cv::Point(f->mvKeyPoints[it->second].pt.x,
//                                              f->mvKeyPoints[it->second].pt.y),8,CV_RGB(255,0,0),2);
//                a++;
//                cv::imshow(to_string(a),g);
//            }
//        }
//        cv::waitKey();
//    }


    std::cout<<"ididididi::::"<<fixed << setprecision(0) << relo_frames[488]->mdPicName <<std::endl;
    pclShow(MapPoints);  ///< 显示所有地图点

    return 0;
}

vector<Relocation_Slam::Frame> read_images_in_folder(cv::String pattern,Eigen::Isometry3d world_camera)
{
//    json files;
//    ofstream os;
//    os.open("test.json");
    vector<cv::String> fn;
//    glob(pattern, fn, false);

    Relocation_Slam::ORBVocabulary* mpvocabulary;
    mpvocabulary = new Relocation_Slam::ORBVocabulary();
    bool bVocLoad = mpvocabulary->loadFromBinaryFile("/home/ubuntu/xd/RelocationSlam/ORBvoc.bin");


    glob(pattern, fn, false);
    vector<Relocation_Slam::Frame> frames;
    size_t count = fn.size(); //number of png files in images folder
    for (size_t i = 0; i < count; i++)
    {
        cv::Mat img = imread(fn[i]);
        string img_stamp(fn[i].substr(24,8));
        int stamp = atoi(img_stamp.c_str());

        //提取坐标
        int first = 0;
        string str_x = "";
        string str_y = "";
        string str_angle = "";
        for(int j=0;j<fn[i].size();j++){
            if(first == 0 && fn[i][j] == '.'){
                first = 1;
                continue;
            }
            if(first == 1 && fn[i][j] == '.'){
                first = 2;
                continue;
            }
            if(first == 1){
                str_x += fn[i][j];
                continue;
            }

            if(first == 2 && fn[i][j] == '.'){
                first = 3;
                continue;
            }
            if(first == 2){
                str_y += fn[i][j];
                continue;
            }

            if(first == 3 && fn[i][j] == '.'){
                first = 4;
                continue;
            }
            if(first == 3){
                str_angle += fn[i][j];
                continue;
            }
        }
        int x = atoi(str_x.c_str());
        int y = atoi(str_y.c_str());
        int angle = atoi(str_angle.c_str());


        Eigen::Isometry3d Tw = Eigen::Isometry3d::Identity();
        Eigen::Vector3d position((double)x/1000,(double)y/1000,0);
        Eigen::AngleAxisd rotation(((double)angle/180*3.14159),Eigen::Vector3d::UnitZ());//绕z轴针旋转
        Tw.pretranslate(position);
        Tw.rotate(rotation);

        Eigen::Isometry3d Tc = Tw * world_camera;

//        json temp;
//        temp["pose_x"] = Tc.translation().x();
//        temp["pose_y"] = Tc.translation().y();
//        files.push_back(temp);

//        std::cout<<"Tw:::"<<std::endl<<Tw.matrix()<<std::endl;
////
//        std::cout<<"Tc:::"<<std::endl<<Tc.matrix()<<std::endl;
////
//        cv::waitKey();

        Relocation_Slam::Frame current_f(mpvocabulary,img,Tc,(double)stamp);
//        std::cout<<"current_f:::"<<current_f.mvKeyPoints.size()<<std::endl;
        frames.push_back(current_f);
    }
//    os<<files;
//    os.close();
    return frames;
}

Eigen::Isometry3d poseTransform(){

    //向base_link下0.3
    Eigen::Isometry3d only_translation = Eigen::Isometry3d::Identity();
    Eigen::Vector3d base_axisX_t = Eigen::Vector3d(0.03,0,0);
    Eigen::Matrix3d rotation_init = Eigen::Matrix3d::Identity();
    only_translation.pretranslate(base_axisX_t);
    only_translation.rotate(rotation_init);

    Eigen::Isometry3d z_180 = Eigen::Isometry3d::Identity();
    Eigen::AngleAxisd z_180_r(3.14159,Eigen::Vector3d::UnitZ());//绕z轴逆时针旋转180
    Eigen::Matrix3d Z_180_r=z_180_r.toRotationMatrix();
    Eigen::Quaterniond Z_180_R(Z_180_r);
    z_180.pretranslate(Eigen::Vector3d(0,0,0));
    z_180.rotate(Z_180_R);

    Eigen::Isometry3d y_57 = Eigen::Isometry3d::Identity();
    Eigen::AngleAxisd y_57_r(-1.00705,Eigen::Vector3d::UnitY());//绕y轴顺时针旋转57.7
    Eigen::Matrix3d Y_57_r=y_57_r.toRotationMatrix();
    Eigen::Quaterniond Y_57_R(Y_57_r);
    y_57.pretranslate(Eigen::Vector3d(0,0,0));
    y_57.rotate(Y_57_R);

    Eigen::Isometry3d x_180 = Eigen::Isometry3d::Identity();
    Eigen::AngleAxisd x_180_r(1.570795,Eigen::Vector3d::UnitY());//绕y轴逆时针旋转90
    Eigen::Matrix3d X_180_r=x_180_r.toRotationMatrix();
    Eigen::Quaterniond X_180_R(X_180_r);
    x_180.pretranslate(Eigen::Vector3d(0,0,0));
    x_180.rotate(X_180_R);

    Eigen::Isometry3d y_180 = Eigen::Isometry3d::Identity();
    Eigen::AngleAxisd y_180_r(-1.570795,Eigen::Vector3d::UnitZ());//绕Z轴顺时针旋转90
    Eigen::Matrix3d Y_180_r=y_180_r.toRotationMatrix();
    Eigen::Quaterniond Y_180_R(Y_180_r);
    y_180.pretranslate(Eigen::Vector3d(0,0,0));
    y_180.rotate(Y_180_R);

    Eigen::Isometry3d base_camera =  only_translation * z_180 * y_57 *x_180 * y_180;

    return base_camera;
}


//int main(){
//    vector<int> test_f; ///<存储测试后选
//    vector<string> vstrImageFilenames;
//    vector<double> vTimestamps;
//    string prefix = "/home/ubuntu/data/rgbd_dataset_freiburg3_long_office_household";
//    string strFile = prefix+"/rgb.txt";
//    string poseFile = prefix+"/groundtruth.txt";
//
//    Relocation_Slam::ORBVocabulary* mpvocabulary;
//    mpvocabulary = new Relocation_Slam::ORBVocabulary();
//    bool bVocLoad = mpvocabulary->loadFromBinaryFile("/home/ubuntu/xd/RelocationSlam/ORBvoc.bin");
//
//    cv::Mat img = cv::imread("/home/ubuntu/data/rgbd_dataset_freiburg3_long_office_household/1341847998.586740.png",CV_LOAD_IMAGE_UNCHANGED);
//    //1.6642 -0.9328 1.5887 0.7066 0.4673 -0.2894 -0.4456
//    Eigen::Isometry3d p_img = Eigen::Isometry3d::Identity();
//    Eigen::Vector3d img_t = Eigen::Vector3d(1.6642,-0.9328,1.5887);
//    Eigen::Quaterniond img_q(-0.4456,0.7066,0.4673,-0.2894);
//    p_img.pretranslate(img_t);
//    p_img.rotate(img_q);
//    double id = 1341847998.586740;
//    Relocation_Slam::Frame img_frame(mpvocabulary,img,p_img,id);
//
//    LoadImages(strFile, vstrImageFilenames, vTimestamps);
//
//    map<double,Eigen::Isometry3d> poseInfo;
//    LoadGroundtruth(poseFile, poseInfo);
//
//    int nImages = vstrImageFilenames.size();
//
//    vector<Relocation_Slam::Frame> KeyFrames;
//    vector<cv::Point3f> m_pOriPtXYZ;
//
//    cv::Mat im;
//    int num=0;
//    for(int ni=0; ni<nImages; ni++)
//    {
//        // Read image from file
//        im = cv::imread(prefix+"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
//        double tframe = floor(vTimestamps[ni] * 100.000f + 0.5) / 100.000f;
//
//        if(poseInfo.count(tframe)){
//            num++;
//            Relocation_Slam::Frame frame(mpvocabulary,im,poseInfo[tframe],vTimestamps[ni]);
//            if(KeyFrames.size()!=0){
//                if(pow((poseInfo[tframe].translation()[0]-KeyFrames.back().mPose.translation().x()),2)
//                   +pow((poseInfo[tframe].translation()[1]-KeyFrames.back().mPose.translation().y()),2)
//                   +pow((poseInfo[tframe].translation()[2]-KeyFrames.back().mPose.translation().z()),2)>0.05*0.05)
//                    KeyFrames.push_back(frame);
//            }
//            else{
//                KeyFrames.push_back(frame);
//            }
//
//            cv::Point3f p;
//            p.x = poseInfo[tframe].translation()[0];
//            p.y = poseInfo[tframe].translation()[1];
//            p.z = poseInfo[tframe].translation()[2];
//            m_pOriPtXYZ.push_back(p);
//        }
//    }
//    std::cout<<"num::"<<num<<endl;
//    std::cout<<"KeyFrames::"<<KeyFrames.size()<<endl;
//
//    cv::Mat K = ( cv::Mat_<double> ( 3,3 ) << 535.4, 0, 320.1, 0, 539.2,247.6, 0, 0, 1 );
////    vector<int> indexs;
////    for(int i=0;i<KeyFrames.size();i++){
////        if(pow(KeyFrames[i].mPose.translation().x()-KeyFrames[50].mPose.translation().x(),2)
////            +pow(KeyFrames[i].mPose.translation().y()-KeyFrames[50].mPose.translation().y(),2)
////            +pow(KeyFrames[i].mPose.translation().z()-KeyFrames[50].mPose.translation().z(),2)<5)
////            indexs.push_back(i);
////    }
//
//    cv::Ptr<cv::DescriptorMatcher> matcher  = cv::DescriptorMatcher::create ( "BruteForce-Hamming" );
//    vector<cv::DMatch> match;
//    vector<cv::DMatch> matches;
//
//
//    vector<Relocation_Slam::Frame*> relo_frames;
//    int frame_id = 1;
////    for(int i=0;i<indexs.size();i++){
//    for(int i=0;i<KeyFrames.size();i++){
////        if(match_points.size()>20){
//        KeyFrames[i].SetFrameId(frame_id);
//        Relocation_Slam::Frame* f = &KeyFrames[i];
//        relo_frames.push_back(f);
//        frame_id++;
////        }
//        if(f->mdPicName>1341847998&&f->mdPicName<1341847999){
//            test_f.push_back(frame_id-1);
//        }
//    }
//
//    std::chrono::time_point<std::chrono::high_resolution_clock> p0 = std::chrono::high_resolution_clock::now();
//
//    std::cout<<"relo_frames.size::"<<relo_frames.size()<<std::endl;
//    vector<Relocation_Slam::MapPoint> MapPoints;
//    //每帧和其前面所有帧进行三角化
//    for(int i=1;i<relo_frames.size();i++){
//        for(int j=0;j<i;j++){ //j 表示 前面的帧
//            vector<pair<int,int>> match_points1; ///< 存储两帧匹配的特征点索引
//            int point_matches =  SelectByBoW(*relo_frames[i],*relo_frames[j],match_points1,1);
//            vector< cv::Point3f > points_r;
//
//            if(match_points1.size()>15)
//                triangulation(relo_frames[i],relo_frames[j],K,match_points1,points_r); ///< 进行三角化
//
//            if(point_matches>15){
//
//                for(int m=0;m<match_points1.size();m++){
//                    if(relo_frames[j]->mbKpTriangle[match_points1[m].second] == false
//                       &&relo_frames[i]->mbKpTriangle[match_points1[m].first] == false){ ///< 若之前帧和当前帧匹配的特征点没被三角化过
//                        cv::Mat p = ( cv::Mat_<float> ( 3,1 ) << points_r[m].x, points_r[m].y, points_r[m].z );
//                        Relocation_Slam::MapPoint mp(p,relo_frames[i],points_r.size(),match_points1[m].first); ///< 创建地图点并添加当前帧观测
//                        mp.AddObservation(relo_frames[j],match_points1[m].second); ///< 为地图点添加之前帧观测
//                        relo_frames[i]->mnMappointId[match_points1[m].first] = MapPoints.size(); ///< 为特征点绑定地图点
//                        relo_frames[j]->mnMappointId[match_points1[m].second] = MapPoints.size();
//                        relo_frames[i]->mbKpTriangle[match_points1[m].first] = true; ///< 设置已经三角化出地图点
//                        relo_frames[j]->mbKpTriangle[match_points1[m].second] = true;
//                        MapPoints.push_back(mp);
//                    }
//                    else if(relo_frames[j]->mbKpTriangle[match_points1[m].second] == true
//                            &&relo_frames[i]->mbKpTriangle[match_points1[m].first] == false){ ///< 之前帧的特征点已经三角化过,需要为该地图点添加当前帧对应特征点作为观测
//                        int point_id = relo_frames[j]->mnMappointId[match_points1[m].second]; ///< 之前帧的特征点对应的地图点ID
//                        MapPoints[point_id].AddObservation(relo_frames[i],match_points1[m].first); ///< 为地图点添加当前帧的观测
//                        relo_frames[i]->mnMappointId[match_points1[m].first] = relo_frames[j]->mnMappointId[match_points1[m].second]; ///< 当前帧的特征点绑定地图点
//                        relo_frames[i]->mbKpTriangle[match_points1[m].first] = true; ///< 当前帧的特征点已经三角化
//                    }
//                    else if(relo_frames[j]->mbKpTriangle[match_points1[m].second] == false
//                            &&relo_frames[i]->mbKpTriangle[match_points1[m].first] == true){ ///< 当前帧的特征点已经三角化过,需要为该地图点添加之前帧对应特征点作为观测
//                        int point_id = relo_frames[i]->mnMappointId[match_points1[m].first]; ///< 当前帧的特征点对应的地图点ID
//                        MapPoints[point_id].AddObservation(relo_frames[j],match_points1[m].second); ///< 为地图点添加之前帧的观测
//                        relo_frames[j]->mnMappointId[match_points1[m].second] = relo_frames[i]->mnMappointId[match_points1[m].first]; ///< 之前帧的特征点绑定地图点
//                        relo_frames[j]->mbKpTriangle[match_points1[m].second] = true; ///< 之前帧的特征点已经三角化
//                    }
//                }
//            }
//
//        }
//    }
//
//    std::cout<<"AllMapPoints.SIZE:::"<<MapPoints.size()<<std::endl;
//
//    vector<Relocation_Slam::MapPoint> MapPoints_c = MapPoints;
//
//    SolveProblem(relo_frames,MapPoints);
//
//    vector<pair<int,int>> match_points2; ///< 存储两帧匹配的特征点索引
//    int point_matches =  SelectByBoW(*relo_frames[test_f[0]],img_frame,match_points2,1);
//
//    cv::Mat R;
//    cv::Mat t;
//    poseEstimate3d2d(
//            *relo_frames[test_f[0]],
//            img_frame,
//            match_points2,
//            MapPoints,
//            K,
//            R,
//            t);
//
//
//    cv::Mat Tinv = (cv::Mat_<double> (4,4) <<
//                                           R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0),
//            R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0),
//            R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0),
//            0,0,0,1);
//    cv::Mat T = Tinv.inv();
//    cout<<"T:"<<endl<<T<<endl;
//
//    std::cout<<"img_frame.T.groundTruth:"<<endl<<p_img.matrix()<<endl;
//
//    std::cout<<relo_frames[test_f[0]]->mdPicName<<",,,,,"<<img_frame.mdPicName<<endl;
//
//    char str1[256];
//    sprintf(str1, "%lf", relo_frames[test_f[0]]->mdPicName);
//    string result1 = str1;
//
//    char str2[256];
//    sprintf(str2, "%lf", img_frame.mdPicName);
//    string result2= str2;
//
////    cout<<result1<<",,,"<<result2<<endl;
//
//    string temp = "/home/ubuntu/data/rgbd_dataset_freiburg3_long_office_household/rgb/";
//    cv::Mat img_11 = cv::imread ( temp+result1+".png", 1 );
//    cv::Mat img_22 = cv::imread ( "/home/ubuntu/data/rgbd_dataset_freiburg3_long_office_household/"+result2+".png", 1 );
//
//    cv::imshow("image_3",img_11);
//    cv::imshow("image_11",img_22);
//    cv::waitKey();
//    pclShow(MapPoints);  ///< 显示所有地图点
//
//    return 0;
//}
